ROS2 TF2 坐标变换

一、为什么需要坐标变换

先看一个具体的场景。假设你的机器人上装了一个激光雷达，装在底盘的前方、上方一点的位置。某一时刻雷达报告："正前方 2 米处有障碍物"。

这句话里的"正前方 2 米"，是相对于雷达自己说的。可你要用它做决策，需要知道的是"这个障碍物在地图上的哪个位置"。两者之间差了一次换算：

雷达坐标系(laser)  --换算-->  底盘坐标系(base_link)  --换算-->  地图坐标系(map)
      ↑                                        ↑
  "2米"是相对这里的                    但你想知道的是这里

这次换算就是坐标变换。机器人上的每个部件——底盘、雷达、摄像头、机械臂的每一节——都有自己的坐标系，TF2 负责维护它们之间的空间关系，并随时回答"A 坐标系里的某个点，在 B 坐标系里是哪儿"。

说明三类典型用途

传感器融合：把激光、摄像头、IMU 的数据统一到同一个坐标系下，才能叠加分析。

运动学：机械臂末端在哪儿，是由各个关节角度逐级累乘算出来的，本质就是一串坐标变换。

导航：目标点在地图上的坐标，要换算成底盘前方多远、偏左偏右多少，轮子才知道怎么转。

二、TF2 的三个核心概念

1. 坐标系（Frame）

坐标系就是一个"带名字的参照系"。每个坐标系有自己的原点朝向，也有一个唯一的名字，比如 base_link、laser。TF2 里所有计算都靠这个名字来指代。

命名不是随便起的，ROS 社区有一套约定（REP-105），常见的有：

坐标系	含义
map	地图坐标系，全局固定，一般认为不漂移
odom	里程计坐标系，连续平滑但会随时间漂移
base_link	机器人本体坐标系，通常定在底盘中心
base_footprint	本体在地面上的投影，z 恒为 0
laser / camera_link / imu_link	各类传感器
link1 … link6 / tool0	机械臂的各段连杆与末端

2. 变换（Transform）

两个坐标系之间的关系用一个变换描述，它由两部分组成：

平移（translation）：x、y、z 三个方向上的偏移，单位米。 旋转（rotation）：用四元数表示，四个分量 x、y、z、w。

注意 TF2 的约定是"由父到子"：说 base_link → laser 的变换，意思是在 base_link 坐标系下，laser 的原点位于何处、朝向如何。

3. TF 树

所有坐标系连起来构成一棵树。这棵树上有一条铁律：

⚠️ 警告每个坐标系只能有一个父节点 可以有任意多个子节点，但父节点只能有一个。否则就不是树而是图，TF2 直接无法工作。实际开发中最常见的翻车方式，就是两个节点分别向同一个坐标系发布它相对不同父节点的变换——比如一个节点说 laser 挂在 base_link 下，另一个说挂在 camera_link 下，TF 树当场崩掉。

4. 静态变换与动态变换

	静态变换	动态变换
适用对象	固定安装、不会动的部件	会运动的部件
话题	/tf_static	/tf
发布频率	只发一次，新节点加入时自动补发	持续高频发布（通常 10~100 Hz）
QoS	transient_local（持久化）	默认 volatile
举例	雷达装在底盘上的位置	机械臂关节角、轮子转速、无人机姿态

这个区分很重要：如果雷达的安装位置是固定的，却用动态方式每秒发几十次，纯属浪费带宽；反过来，关节角如果是静态的，那机器人就永远动不了了。

三、旋转的表示：为什么是四元数

描述旋转有两种常见方式：

欧拉角（roll、pitch、yaw）——分别绕 x、y、z 轴转多少角度。非常直观，人类容易理解。

四元数（x、y、z、w）——四个数表示一个旋转。不直观，但没有万向锁问题，插值和计算都更稳定。

说明万向锁 用欧拉角表示旋转时，当 pitch 转到 ±90°，roll 和 yaw 会退化成同一个自由度，丢失一个方向的控制能力——这就是万向锁。ROS2 内部统一用四元数，正是为了避开它。你可以在命令行里用欧拉角，但写进代码的消息里一定是四元数。

欧拉角转四元数的公式（写节点时经常需要）：

import math


def quaternion_from_euler(roll, pitch, yaw):
    """欧拉角转四元数，输入单位：弧度，返回 (x, y, z, w)"""
    cy = math.cos(yaw * 0.5)
    sy = math.sin(yaw * 0.5)
    cp = math.cos(pitch * 0.5)
    sp = math.sin(pitch * 0.5)
    cr = math.cos(roll * 0.5)
    sr = math.sin(roll * 0.5)

    x = sr * cp * cy - cr * sp * sy
    y = cr * sp * cy + sr * cp * sy
    z = cr * cp * sy - sr * sp * cy
    w = cr * cp * cy + sr * sp * sy
    return (x, y, z, w)

如果不想手写，也可以安装 tf_transformations 这个包直接调用：

sudo apt install ros-jazzy-tf-transformations

from tf_transformations import quaternion_from_euler

q = quaternion_from_euler(0, 0, 1.57)   # 绕 z 轴转 90 度
print(q)   # (0.0, 0.0, 0.7071067811865475, 0.7071067811865476)

注意：纯绕 z 轴转 90°（π/2），四元数是 (0, 0, sin(π/4), cos(π/4)) ≈ (0, 0, 0.707, 0.707)。记住这个特例，调试时能一眼看出旋转对不对。

四、TF2 命令行工具

排查坐标问题时，命令行工具比写代码快得多。这几个是必会的：

命令	作用
ros2 run tf2_tools view_frames	采集 5 秒数据，生成 frames.pdf，直观展示整棵坐标树
ros2 run tf2_ros tf2_echo 源 目标	实时打印两个坐标系之间的变换，持续刷新
ros2 run tf2_ros tf2_monitor	统计各个变换的发布频率、延迟、最坏情况
ros2 run tf2_ros static_transform_publisher …	发布一个静态变换，临时补上缺失的环节
ros2 topic echo /tf_static	直接查看静态变换的原始消息

1. view_frames：先看树长什么样

ros2 run tf2_tools view_frames

它会监听 5 秒，然后在当前目录生成 frames.pdf（和 frames.gv）。打开 PDF 就能看到整棵树、每个变换的发布频率和最近一次的时间戳。

注意 view_frames 依赖 graphviz，如果报错先装：sudo apt install graphviz。

2. tf2_echo：实时看两个坐标系的关系

# 语法：tf2_echo <源坐标系> <目标坐标系>
ros2 run tf2_ros tf2_echo base_link laser

含义是"在 base_link 看来，laser 在哪儿"。输出会持续刷新，包含平移（Translation）和旋转（Rotation）。

如果报 Invalid frame ID，说明这两个坐标系之间的链路还没建立起来，或者名字拼错了。

3. static_transform_publisher：手工补一个变换

这是调试时的万能补丁。比如你想验证"如果雷达装在底盘前方 0.3 米、上方 0.2 米"，可以手工发一个：

ros2 run tf2_ros static_transform_publisher \
  --x 0.3 --y 0.0 --z 0.2 \
  --qx 0 --qy 0 --qz 0 --qw 1 \
  --frame-id base_link --child-frame-id laser

也可以用欧拉角代替四元数，同样三个方向：

ros2 run tf2_ros static_transform_publisher \
  --x 0.3 --y 0.0 --z 0.2 \
  --roll 0.0 --pitch 0.0 --yaw 0.0 \
  --frame-id base_link --child-frame-id laser

⚠️ 警告两种写法不能混用 四元数（--qx --qy --qz --qw）和欧拉角（--roll --pitch --yaw）只能选一种，同时给出会报错。另外欧拉角的单位是弧度，不是角度——想转 90 度要写 1.5708 而不是 90。

顺带一提：旧版本 ROS2 用的是九个位置参数（static_transform_publisher x y z yaw pitch roll frame_id child_frame_id），这个写法已经废弃，新版本会直接报错。网上很多老教程还在用，注意鉴别。

五、学习内容：发布与监听变换

下面我们写一组节点：一个把"雷达相对底盘"的变换持续发布出去，另一个接收并查询它。这是 TF2 最基础的一对操作。

（一）创建功能包

cd ~/ros_code/src
ros2 pkg create --build-type ament_python tf2_demo \
  --dependencies rclpy tf2_ros geometry_msgs

关键是依赖里的 tf2_ros（TF2 的 ROS 接口）和 geometry_msgs（变换消息类型所在）。

（二）编写 TF 发布者节点

在 tf2_demo/tf2_demo/ 目录下新建 tf2_broadcaster.py：

import math

import rclpy
from rclpy.node import Node
from geometry_msgs.msg import TransformStamped
from tf2_ros import TransformBroadcaster


class TFBroadcaster(Node):

    def __init__(self):
        super().__init__('tf_broadcaster')

        # 创建广播器
        self.broadcaster = TransformBroadcaster(self)

        # 以 50 Hz（每 0.02 秒）发布一次
        self.timer = self.create_timer(0.02, self.publish_transform)
        self.angle = 0.0

    def publish_transform(self):
        t = TransformStamped()

        # 时间戳：必须填当前时刻
        t.header.stamp = self.get_clock().now().to_msg()

        # 父子坐标系
        t.header.frame_id = 'base_link'
        t.child_frame_id = 'laser'

        # 平移：雷达装在底盘前方 0.3 m、上方 0.2 m
        t.transform.translation.x = 0.3
        t.transform.translation.y = 0.0
        t.transform.translation.z = 0.2

        # 旋转：这里让它绕 z 轴缓慢转动，用来演示"动态变换"
        # 绕单轴转 theta 角，四元数为 (0, 0, sin(theta/2), cos(theta/2))
        self.angle += 0.01
        t.transform.rotation.x = 0.0
        t.transform.rotation.y = 0.0
        t.transform.rotation.z = math.sin(self.angle / 2.0)
        t.transform.rotation.w = math.cos(self.angle / 2.0)

        # 发布
        self.broadcaster.sendTransform(t)


def main():
    rclpy.init()
    node = TFBroadcaster()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    rclpy.shutdown()


if __name__ == '__main__':
    main()

几处要留意：

时间戳必须填。header.stamp 用 self.get_clock().now().to_msg() 取当前时刻，漏填或填 0 会导致监听端查不到。

frame_id 是父，child_frame_id 是子。这里表示"在 base_link 下，laser 位于 (0.3, 0, 0.2)"。写反了整棵树就颠倒了。

旋转用四元数。绕单根轴转 θ 角的四元数有个简便写法：绕 z 轴是 (0, 0, sin(θ/2), cos(θ/2))，绕 x 轴是 (sin(θ/2), 0, 0, cos(θ/2))。

（三）编写 TF 监听者节点

同目录下新建 tf2_listener.py：

import rclpy
from rclpy.node import Node
from rclpy.time import Time
from tf2_ros import Buffer, TransformListener


class TFListener(Node):

    def __init__(self):
        super().__init__('tf_listener')

        # Buffer 负责缓存收到的所有变换（默认缓存 10 秒）
        self.buffer = Buffer()

        # Listener 负责订阅 /tf 和 /tf_static，并往 Buffer 里塞数据
        self.listener = TransformListener(self.buffer, self)

        # 每秒查询一次
        self.timer = self.create_timer(1.0, self.query_transform)

    def query_transform(self):
        try:
            # 查询：在 base_link 下，laser 的位置和朝向
            # 参数顺序是 (目标坐标系, 源坐标系, 时间)
            trans = self.buffer.lookup_transform(
                'base_link',
                'laser',
                Time()          # Time() 表示"用最新的可用数据"
            )
        except Exception as e:
            # 最常见的失败原因是数据还没到，等下一次定时器
            self.get_logger().warn(f'查询失败：{e}')
            return

        t = trans.transform.translation
        r = trans.transform.rotation
        self.get_logger().info(
            f'base_link -> laser  '
            f'平移=({t.x:.3f}, {t.y:.3f}, {t.z:.3f})  '
            f'旋转=({r.x:.3f}, {r.y:.3f}, {r.z:.3f}, {r.w:.3f})'
        )


def main():
    rclpy.init()
    node = TFListener()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    rclpy.shutdown()


if __name__ == '__main__':
    main()

说明Buffer 和 Listener 的分工Buffer 是仓库，缓存所有收到的变换；TransformListener 是搬运工，负责订阅话题并往仓库里放数据。查询时你只跟 Buffer 打交道。这个设计的好处是：即使某个变换是几秒前发布的，只要还在缓存窗口（默认 10 秒）内，你依然能查到。

lookup_transform 的参数顺序容易记反，记住它是"从源坐标系看，目标坐标系在哪"的语义：lookup_transform('base_link', 'laser', ...) 得到的是"laser 在原点的 base_link 坐标系里的位姿"。换个说法：把 base_link 当基准，laser 的坐标是多少。

（四）修改 setup.py

    entry_points={
        'console_scripts': [
            'tf2_broadcaster = tf2_demo.tf2_broadcaster:main',
            'tf2_listener = tf2_demo.tf2_listener:main',
        ],
    },

（五）修改 package.xml

在 <package> 标签内确认有这三行（ros2 pkg create 带 --dependencies 时通常已经自动加好了）：

  <depend>rclpy</depend>
  <depend>tf2_ros</depend>
  <depend>geometry_msgs</depend>

（六）编译并运行

cd ~/ros_code
colcon build --packages-select tf2_demo
source install/setup.bash

终端一，启动发布者：

ros2 run tf2_demo tf2_broadcaster

终端二（记得先 source install/setup.bash），启动监听者：

ros2 run tf2_demo tf2_listener

应该能看到每秒输出一次变换数据。同时观察发布者那个终端——可以试着把发布者的定时器改成 1 秒一次，再启动监听着看，会发现监听者刚开始会打几条"查询失败"的警告，然后才恢复正常。这正是 try/except 存在的意义。

（七）用命令行工具验证

再开一个终端：

# 看二进制的变换消息
ros2 topic echo /tf --once

# 实时查看 base_link 到 laser 的变换
ros2 run tf2_ros tf2_echo base_link laser

# 查看整棵树（会在当前目录生成 frames.pdf）
ros2 run tf2_tools view_frames

# 确认话题是否真的在发
ros2 topic hz /tf

这一环节特别重要：凡是坐标相关的怪问题，第一步永远是 view_frames 看一眼树对不对。多数时候问题就明摆在那儿——要么少了某个环节，要么某个坐标系有两个爹。

六、常见问题排查

报错信息	原因与处理
... target_frame does not exist	该坐标系从未被任何节点发布过。用 view_frames 确认树里到底有没有它，再检查名字是否拼错（大小写敏感）。
... source_frame does not exist	同上，通常是自己写错名字，或者发布该坐标系的节点没启动。
Lookup would require extrapolation into the future	请求的时间戳比 Buffer 里最新的数据还新。常见于查询时传了一个未来时间，或者两个节点的时钟不同步。
Lookup would require extrapolation into the past	请求的时间戳太旧，已经超出 Buffer 的缓存窗口（默认 10 秒）。要么提高发布频率，要么别用太老的时间戳。
刚开始查询就持续失败	时序问题。监听者启动时 Buffer 还是空的，等广播者发几次数据就好了。代码里用 try/except 兜住，别让它把节点冲垮。
view_frames 生成的图里某个环节是断的	说明该环节没人发布。检查对应节点、检查它是不是发到了别的坐标系名字上。
树里出现同一坐标系有两个父节点	非法结构。查找是哪两个节点在打架，通常其中一个应该改成 --child-frame-id 指向别的名字。

说明本节小结 坐标变换是机器人开发的骨架。TF2 用"一棵树"描述所有部件之间的空间关系，靠 /tf（动态）和 /tf_static（静态）两个话题维护这棵树。

写代码时记住四件事：时间戳要填、父子别写反、旋转用四元数、查询要 try/except。

下一节我们给机器人建一个"身体"——用 URDF 描述它的连杆和关节，这样前面这些坐标系才有实际对应的部件。